// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CameraPoseEstimationTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Vision
{
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Camera pose estimation tests.
    /// </summary>
    [TestClass]
    public class RobotPoseEstimationTest
    {
        /// <summary>
        /// Planar Pose Reference input.
        /// X offset (meters), Y offset (meters), Heading (Radians)
        /// </summary>
        private static readonly Pose2D[] PlanarPoseReferenceInput = 
        {
            new Pose2D(0.00176, -0.0009, 0.0033161),
            new Pose2D(-0.3045, -0.1655, 1.5725),
            new Pose2D(-0.1279, -0.4927, 3.1398),
            new Pose2D(0.18733, -0.3018, -1.5638),
            new Pose2D(-0.0516, -0.507, 0),
            new Pose2D(0.2043, -0.2043, 1.5656),
            new Pose2D(-0.0705, 0.0095, 3.1364),
            new Pose2D(-0.3185, -0.2214, -1.583),
            new Pose2D(-0.37455, 0.03636, 0),
            new Pose2D(-0.3427, -0.5445, 1.5708),
            new Pose2D(0.2479, -0.536, -3.1316),
            new Pose2D(0.2234, 0.0835, -1.5691),
        };

        /// <summary>
        /// Camera Pose Reference Results for each input.
        /// X offset (meters), Y offset (meters), Heading (Radians)
        /// </summary>
        private static readonly Pose2D[] CameraPoseReferenceResults = 
        {
            new Pose2D(0.000906, 0.001757, -0.0033161),
            new Pose2D(-0.304782, -0.164981, -1.5725),
            new Pose2D(-0.492928, 0.127016, -3.1398),
            new Pose2D(-0.185214, 0.303103, 1.5638),
            new Pose2D(0.507, -0.0516, 0),
            new Pose2D(0.205359, -0.203236, -1.5656),
            new Pose2D(0.009134, 0.070548, -3.1364),
            new Pose2D(0.315774, 0.225270, 1.583),
            new Pose2D(-0.03636, -0.37455, 0),
            new Pose2D(-0.342702, -0.544499, -1.5708),
            new Pose2D(-0.538450, -0.242532, 3.1316),
            new Pose2D(-0.223541, -0.083121, 1.5691),
        };

        /// <summary>
        /// Tests robot pose estimation by performing it on a reference robot offset and comparing the results.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CameraPoseEstimationTest()
        {              
            for (int i = 0; i < PlanarPoseReferenceInput.Length; ++i)
            {
                this.VerifyCameraPoseEstimatorResults(PlanarPoseReferenceInput[i], CameraPoseReferenceResults[i]);
            }
        }

        /// <summary>
        /// Verifies the camera pose for the given planar pose with reference camera pose.
        /// </summary>
        /// <param name="planarPose">Planar Pose</param>
        /// <param name="referenceCameraPose">Reference camera pose</param>
        private void VerifyCameraPoseEstimatorResults(Pose2D planarPose, Pose2D referenceCameraPose)
        {
            Pose2D? resultCameraPose = UpwardFacingCameraPoseEstimator.ConvertPlanarCoordinateToLandmarkCoordinate(planarPose);            
            Assert.IsNotNull(resultCameraPose);

            // This is useful to generate the reference poses with additional data.
            ////TraceOut.Info("new Pose2D({0}, {1}, {2}),", resultCameraPose.Value.X, resultCameraPose.Value.Y, resultCameraPose.Value.Heading);
            
            Assert.AreEqual(referenceCameraPose.X, resultCameraPose.Value.X, MathConstants.ErrorEpsilon);
            Assert.AreEqual(referenceCameraPose.Y, resultCameraPose.Value.Y, MathConstants.ErrorEpsilon);
            Assert.AreEqual(referenceCameraPose.Heading, resultCameraPose.Value.Heading, MathConstants.ErrorEpsilon);
        }
    }
}
